;;----------- solve non-linear eqns by Levenberg-Marquardt method

;; load Least Squares method to solve d
(load "/home/cl/gits/scheme/numerical-analysis/linear-eqns/least-squares.scm")
;; load Gauss-Siedel method to solve d (似乎不稳定??)
;;(load "/home/cl/gits/scheme/numerical-analysis/linear-eqns/iterative-methods.scm")

;; load matrix oprations
(load "/home/cl/gits/scheme/numerical-analysis/matrix-vector-operations.scm")

;; import stream
(library-directories '("."  "/home/cl/gits/libs-for-chezscheme"))
(import (stream)
        (range)
        (parallelization-in-vector))

;; get Gamma, H, and f(x1)
(define (GammaT-H-fx1 x0 x1 f n)
  (let* ([H (vector-map - x0 x1)];; H is a diag-matrix
         [xi-func (lambda (j)
                    (let ([vec (vector-copy x1)])
                      (vector-set! vec j (+ (vector-ref vec j)
                                            (vector-ref H j)))
                      vec))]
         [f-of-x1 (f x1)]
         [GammaT (vector-map (lambda (j)
                               (vector-map - (f (xi-func j)) f-of-x1))
                              (vector-range n))])
    (values GammaT H f-of-x1)))

;; next approximate solution
(define (nextsol x0 x1 f n)
  (let-values ([(GammaT H f-of-x1) (GammaT-H-fx1 x0 x1 f n)])
    (let* ([A (matrix-*-matrix GammaT
                               (matrix-transport GammaT))]
           [b (matrix-*-vector GammaT f-of-x1)])
      (do ((i 0 (+ i 1)))
          ((= i n))
        (matrix-set! A i i (+ (matrix-ref A i i)
                              (* (vector-norm f-of-x1)
                                 (expt (vector-ref H i) 2)))))
      (cholesky-decomposition! A n); A is now the L(lower-triangular matrix)
      
      (let ([minus-d (vector-map * H
                                 (sol-trans-lower-triangular-eqns!
                                  A (sol-lower-triangular-eqns! A b n) n))])
        (if (nan? (vector-norm minus-d))
            #f
            (vector-map - x1 minus-d))))))

;; LM method
(define (levenberg-marquardt-stream f x0 x1 n)
  (stream-cons x0
               (let ([x2 (nextsol x0 x1 f n)])
                 (if x2
                     (levenberg-marquardt-stream f x1 x2 n)
                     stream-null)))
  )


#| ;;test

(let* ([f (lambda (x)
            (vector (- (* 3 (vector-ref x 0))
                       (cos (* (vector-ref x 1) (vector-ref x 2)))
                       0.5)
                    (+ (expt (vector-ref x 0) 2)
                       (* -81 (expt (+ 0.1 (vector-ref x 1)) 2))
                       (sin (vector-ref x 2))
                       1.06)
                    (+ (exp (* -1 (vector-ref x 0) (vector-ref x 1)))
                       (* 20 (vector-ref x 2))
                       (* 10/3 3.1415926)
                       -1)))]
       [x0 (vector 0.0 0.0 0.0)]
       [x1 (vector 0.01 0.01 0.01)]
       [n 3]
       [solutions (levenberg-marquardt-stream f x0 x1 n)])
  (for-each (lambda (i)
              (display "solution is: ")
              (display (stream-ref solutions i))
              (newline))
            (range 9))
  )

|#







